# -*- coding: utf-8 -*-

import sys 
from datetime import datetime 
from timeit import default_timer as timer
from loguru import logger as log
from PyQt5 import QtCore, QtGui, QtWidgets
from PyQt5.QtWidgets import QMainWindow , QApplication
from PyQt5.QtGui import QIntValidator
from PyQt5.QtCore import QTimer, QThread, pyqtSignal
from main_dialog import Ui_MainWindow
from main_module import *
from sacp_robot import *


class MainWindow(QMainWindow, Ui_MainWindow):
    APP_NAME = "VacuumTester"
    APP_VERSION = "V1.0"

    ## 设备状态
    STATE_COM_CLOSED = 0
    STATE_DEVICE_INACTIVE = 1
    STATE_DEVICE_ACTIVE = 2

    def __init__(self):
        super(MainWindow, self).__init__()
        self.setupUi(self)        
        self.setWindowTitle("%s %s" % (self.APP_NAME, self.APP_VERSION))
        
        ## 启动日志子模块
        self.uiLog = UiLog(False)
        self.uiLog.messageReceived.connect(self.updateLog)
        self.uiLog.start()
                        
        ## 初始化所有指示器 
        self.ledComState = Indicator(self.indicatorComState)        
        self.ledLinkState = Indicator(self.indicatorLinkState)
        self.ledDustBox = Indicator(self.indicatorDustBox)
        self.ledCollisionFront = Indicator(self.indicatorCollisionFront, reverse=True)
        self.ledCollisionLeft = Indicator(self.indicatorCollisionLeft, reverse=True)
        self.ledCollisionRight = Indicator(self.indicatorCollisionRight, reverse=True)
        self.ledMopState = Indicator(self.indicatorMopState)
        self.ledOnCharger = Indicator(self.indicatorOnCharger)
        self.ledLeftWheelOff = Indicator(self.indicatorLeftWheelOff, reverse=True)
        self.ledRightWheelOff = Indicator(self.indicatorRightWheelOff, reverse=True)
        self.ledCollisionLidar = Indicator(self.indicatorCollisionLidar, reverse=True)
        self.ledCharging = Indicator(self.indicatorCharging)
        self.ledWaterTank = Indicator(self.indicatorWaterTank)
        self.ledUpgradeRequest = Indicator(self.indicatorUpgradeRequest)        

        self.ledGdFront = Indicator(self.indicatorGdFront, reverse=True)
        self.ledGdLeft = Indicator(self.indicatorGdLeft, reverse=True)
        self.ledGdRight = Indicator(self.indicatorGdRight, reverse=True)  

        self.labelStatus1.setText("")
        self.labelStatus2.setText("")
        self.labelStatus3.setText("")               

        ## 初始化数据集
        self.db = Database()

        # self.db.set(SacpId.ATTR_SOFTWARE_VERSION, 0x00010003)
        # self.db.set(SacpId.ATTR_SOFTWARE_RELEASED_TIME, 1385629728)

        ## UI信号处理对接
        self.buttonOpenCom.clicked.connect(self.onButtonOpenComClicked)
        self.buttonClearLog.clicked.connect(self.clearLog)
        
        ## 连接管理状态 
        self.comOpened = False 
        self.deviceState = self.STATE_COM_CLOSED
        ## 创建一个SACP的实例
        self.sacpRobot = SacpRobot(self.uiLog, self.db) 
        
        ## 创建一个定时器，每隔500ms刷新设备活动状态
        self.deviceLinkStateTimer = QTimer()
        self.deviceLinkStateTimer.timeout.connect(self.deviceLinkStateTimerHandle)
        self.deviceLinkStateTimer.start(500)
        
        self.deviceDataRefreshTimer = QTimer()
        self.deviceDataRefreshTimer.timeout.connect(self.deviceDataRefreshHandle)
        self.deviceDataRefreshTimer.start(200)
        
        ## 加载设置项
        self.setting = Setting('config.yaml')
        self.setting.serialChanged.connect(self.updateSerialPorts)
        self.updateSerialPorts()
        port,rate = self.setting.getSerialConfig()
        self.comboBoxComPort.setCurrentText(port)
        self.comboBoxBaudrate.setCurrentText(rate)
        self.comboBoxComPort.currentTextChanged.connect(self.onComPortChanged)
        self.comboBoxBaudrate.currentTextChanged.connect(self.onComBaudrateChanged)
        ## 初始状态为关闭状态
        self.onComPortClosed()

        ## 初始化控制控件, 
        # PWM调节器
        self.pwmMainBrush = PwmRegultor(self.sliderMainBrush, self.labelMainBrushValue, SacpId.ATTR_MAIN_BRUSH_SET_PWM)
        self.pwmLeftBrush = PwmRegultor(self.sliderLeftBrush, self.labelLeftBrushValue, SacpId.ATTR_LEFT_SIDE_BRUSH_SET_PWM)
        self.pwmRightBrush = PwmRegultor(self.sliderRightBrush, self.labelRightBrushValue, SacpId.ATTR_RIGHT_SIDE_BRUSH_SET_PWM)
        self.pwmFanControl = PwmRegultor(self.sliderFanControl, self.labelFanControlValue, SacpId.ATTR_FAN_SET_PWM)
        self.pwmMainBrush.valueChanged.connect(self.pwmRegultorApply)
        self.pwmLeftBrush.valueChanged.connect(self.pwmRegultorApply)
        self.pwmRightBrush.valueChanged.connect(self.pwmRegultorApply)
        self.pwmFanControl.valueChanged.connect(self.pwmRegultorApply)
        
        ## 方向键处理
        self.KEY_UP = 1
        self.KEY_DOWN = 2
        self.KEY_RIGHT = 3
        self.KEY_LEFT = 4
        self.KEY_OK = 5
        self.extButtonOk = ExtButton(self.buttonConfirm, self.KEY_OK)
        self.extButtonUp = ExtButton(self.buttonUp, self.KEY_UP)
        self.extButtonDown = ExtButton(self.buttonDown, self.KEY_DOWN)
        self.extButtonRight = ExtButton(self.buttonRight, self.KEY_RIGHT)
        self.extButtonLeft = ExtButton(self.buttonLeft, self.KEY_LEFT)
        
        self.extButtonOk.keyEvent.connect(self.dirButtonHandle)
        self.extButtonUp.keyEvent.connect(self.dirButtonHandle)
        self.extButtonDown.keyEvent.connect(self.dirButtonHandle)
        self.extButtonRight.keyEvent.connect(self.dirButtonHandle)
        self.extButtonLeft.keyEvent.connect(self.dirButtonHandle)        
                
        # 初始化PID控制器
        self.pidTargets = self.setting.getPidTargets()
        self.pidRanges = self.setting.getPidRanges()        
        
        targets = []
        for t in self.pidTargets:
            targets.append(t['name'])        
        self.comboBoxPidType.addItems(targets)
        self.comboBoxPidType.currentIndexChanged.connect(self.onPidTargetChanged)
        self.pidKp = None 
        self.pidKi = None 
        self.pidKd = None 
        self.pidLimited = None
        self.pidControl = None
        self.radioPid.setChecked(True)
        self.radioPwm.setChecked(False)        
        self.radioPid.toggled.connect(self.onPidControlTypeChanged)
        self.radioPid.toggled.connect(self.onPidControlTypeChanged)
        self.onPidTargetChanged(0)
        
        self.buttonReadPid.clicked.connect(self.tryReadPidParameters)
        self.buttonWritePid.clicked.connect(self.tryWritePidParameters)  
        self.buttonDataTxPause.clicked.connect(self.onButtonTxPauseClicked)      
        
        ## 初始化数据可视化控件
        self.checkBoxDataVisual.setChecked(False)
        self.editTcpPort.setText(str(self.setting.getDataVisualIpPort()))
        self.editPeriod.setText(str(self.setting.getDataVisualTxPeriod()))
        ids,options = self.setting.getDataVisualOptions()
        chAttrIds = self.setting.getDataVisualChannelConfigs()
        self.dataVisualAttributeIds = ids
        self.setupDataVisualChannel(self.comboBoxDataCh1, ids, options, chAttrIds[0])        
        self.setupDataVisualChannel(self.comboBoxDataCh2, ids, options, chAttrIds[1])  
        self.setupDataVisualChannel(self.comboBoxDataCh3, ids, options, chAttrIds[2])  
        self.setupDataVisualChannel(self.comboBoxDataCh4, ids, options, chAttrIds[3])  
        self.setupDataVisualChannel(self.comboBoxDataCh5, ids, options, chAttrIds[4])  
        self.setupDataVisualChannel(self.comboBoxDataCh6, ids, options, chAttrIds[5])  
        self.setupDataVisualChannel(self.comboBoxDataCh7, ids, options, chAttrIds[6])  
        self.setupDataVisualChannel(self.comboBoxDataCh8, ids, options, chAttrIds[7])  
        self.checkBoxDataVisual.toggled.connect(self.onDataVisualEnableChanged)
        self.dataVisualConfigTimer = QTimer()
        self.editTcpPort.textChanged.connect(self.onDataVisualIpPortChanged)
        self.editPeriod.textChanged.connect(self.onDataVisualIntervalChanged)
        self.dataVisualConfigTimer.timeout.connect(self.saveDataVisualConfig)

        ## 默认VOFA服务器为空
        self.vofaServer = VofaServer(self.db)

        ## 对接小程序的网络遥控器
        self.ipRemoter = IpRemoter(uiLog=self.uiLog)
        if not self.ipRemoter.start():
            self.log("网络遥控器服务启动失败")
        self.ipRemoter.commandReceived.connect(self.onIpRemoterCommandReceived)
        
        ## 机器参数设置
        self.buttonReadParameter.clicked.connect(self.tryReadMachineParameters)
        self.buttonWriteParameter.clicked.connect(self.tryWriteMachineParameters)

        # self.testTimer = QTimer()
        # self.testTimer.timeout.connect(self.test1)
        # self.testTimer.start(10)

    def onIpRemoterCommandReceived(self, command):
        self.log("收到遥控器命令:%s" % command)

    def test1(self, val=''):
        log.info("rx msg=%s" % val)
                        
    def test(self, val = 0):
        log.info("test val=%d" % val)
        
    def log(self, msg):
        """日志函数，写一条日志

        Args:
            msg (_type_): 日志信息
        """
        self.uiLog.log(msg)

    def updateLog(self, msg):
        """日志更新函数 

        Args:
            msg (_type_): 日志信息
        """
        self.editLog.append(msg)
        
    def clearLog(self):
        self.editLog.clear()
        self.sacpRobot.clear_counters()

    def closeEvent(self, event):
        """窗口关闭的钩子函数

        Args:
            event (_type_): _description_
        """
        if self.comOpened:
            if self.sacpRobot.is_running():
                self.sacpRobot.stop()
        if self.ipRemoter.is_running():
            self.ipRemoter.stop()
        if self.vofaServer.is_running():
            self.vofaServer.stop()

    def deviceDataRefreshHandle(self):
        ## 只在设备活动时，展现设备相关数据 
        if self.deviceState == self.STATE_DEVICE_ACTIVE:
            self.updateLeftWheel()
            self.updateRightWheel()
            self.updateImuAndOdometry()
            self.updateMotorInfo()
            self.updateMachineState()
            self.updateGroundDetectState()

        ## 展示统计数据 
        self.labelStatus1.setText("RP:%d" % self.sacpRobot.counters.report_frames)
        self.labelStatus2.setText("R:%d RF:%d" % (self.sacpRobot.counters.read_frames, self.sacpRobot.counters.read_failed_frames))
        self.labelStatus3.setText("W:%d WF:%d" % (self.sacpRobot.counters.write_frames, self.sacpRobot.counters.write_failed_frames))

    def updateDeviceInfo(self):
        if self.deviceState != self.STATE_DEVICE_ACTIVE:
            self.labelSoftwareVersionValue.setText("")
            self.labelUpTimeValue.setText("")
            self.labelRtcTimeValue.setText("")
            self.labelVoltageValue.setText("")
            self.labelReleasedDateValue.setText("")
            self.labelMemUsagedValue.setText("")            
            return                     
        ver = self.db.get(SacpId.ATTR_SOFTWARE_VERSION, 0)
        released = self.db.get(SacpId.ATTR_SOFTWARE_RELEASED_TIME, 0)
        voltage = self.db.get(SacpId.ATTR_BATTERY_VOLTAGE, 0)
        battery_level = self.db.get(SacpId.ATTR_BATTERY_LEVEL, 0)
        uptime = self.db.get(SacpId.ATTR_UP_TIME, 0)
        rtctime = self.db.get(SacpId.ATTR_RTC_TIME, 0)
        mem_total = self.db.get(SacpId.ATTR_MEM_TOTAL, 0)
        mem_free = self.db.get(SacpId.ATTR_MEM_FREE, 0)

        self.labelSoftwareVersionValue.setText(to_version(ver))
        self.labelUpTimeValue.setText(to_uptime(uptime))
        self.labelRtcTimeValue.setText(to_time_string(rtctime))
        self.labelVoltageValue.setText("%.1fV/%.1f%%" % (voltage, battery_level))
        self.labelReleasedDateValue.setText(to_time_string(released))
        self.labelMemUsagedValue.setText("%dKB/%dKB" % (mem_free, mem_total))

    def updateLeftWheel(self):
        current = self.db.get(SacpId.ATTR_LEFT_WHEEL_CURRENT, 0)
        velocity = self.db.get(SacpId.ATTR_LEFT_WHEEL_VELOCITY, 0)
        pidTarget = self.db.get(SacpId.ATTR_LEFT_WHEEL_PID_TARGET, 0)
        pidOutput = self.db.get(SacpId.ATTR_LEFT_WHEEL_PID_OUTPUT, 0)
        pidError = self.db.get(SacpId.ATTR_LEFT_WHEEL_PID_ERROR, 0)
        self.labelLWheelCurrentValue.setText(to_float_string(current))
        self.labelLWheelVelocityValue.setText(to_float_string(velocity))
        self.labelLWheelTargetVelocityValue.setText(to_float_string(pidTarget))
        self.labelLWheelOutputVelocityValue.setText(to_float_string(pidOutput))
        self.labelLWheelErrorValue.setText(to_float_string(pidError))

    def updateRightWheel(self):
        current = self.db.get(SacpId.ATTR_RIGHT_WHEEL_CURRENT, 0)
        velocity = self.db.get(SacpId.ATTR_RIGHT_WHEEL_VELOCITY, 0)
        pidTarget = self.db.get(SacpId.ATTR_RIGHT_WHEEL_PID_TARGET, 0)
        pidOutput = self.db.get(SacpId.ATTR_RIGHT_WHEEL_PID_OUTPUT, 0)
        pidError = self.db.get(SacpId.ATTR_RIGHT_WHEEL_PID_ERROR, 0)
        self.labelRWheelCurrentValue.setText(to_float_string(current))
        self.labelRWheelVelocityValue.setText(to_float_string(velocity))
        self.labelRWheelTargetVelocityValue.setText(to_float_string(pidTarget))
        self.labelRWheelOutputVelocityValue.setText(to_float_string(pidOutput))
        self.labelRWheelErrorValue.setText(to_float_string(pidError))
        
    def updateImuAndOdometry(self):
        gx = self.db.get(SacpId.ATTR_IMU_GYROSCOPE_X, 0)
        gy = self.db.get(SacpId.ATTR_IMU_GYROSCOPE_Y, 0)
        gz = self.db.get(SacpId.ATTR_IMU_GYROSCOPE_Z, 0)
        ax = self.db.get(SacpId.ATTR_IMU_ACCELEROMETER_X, 0)
        ay = self.db.get(SacpId.ATTR_IMU_ACCELEROMETER_Y, 0)
        az = self.db.get(SacpId.ATTR_IMU_ACCELEROMETER_Z, 0)
        ox = self.db.get(SacpId.ATTR_ODOMETRY_X, 0)
        oy = self.db.get(SacpId.ATTR_ODOMETRY_Y, 0)
        oz = self.db.get(SacpId.ATTR_ODOMETRY_Z, 0)
        yaw = self.db.get(SacpId.ATTR_ODOMETRY_YAW, 0)
        pitch = self.db.get(SacpId.ATTR_ODOMETRY_PITCH, 0)
        roll = self.db.get(SacpId.ATTR_ODOMETRY_ROLL, 0)
        gv = "X:%.3f Y:%.3f Z:%.3f" % (gx, gy, gz)
        av = "X:%.3f Y:%.3f Z:%.3f" % (ax, ay, az)
        ov = "X:%.3f Y:%.3f Z:%.3f" % (ox, oy, oz)
        ev = "Y:%.3f P:%.3f R:%.3f" % (yaw, pitch, roll)
        
        self.labelGyroscopeValue.setText(gv)
        self.labelAccelerometerValue.setText(av)
        self.labelOdometryValue.setText(ov)
        self.labelEulerAnglesValue.setText(ev)
        
    def updateMotorInfo(self):
        mb = self.db.get(SacpId.ATTR_MAIN_BRUSH_CURRENT, 0)
        lb = self.db.get(SacpId.ATTR_LEFT_SIDE_BRUSH_CURRENT, 0)
        rb = self.db.get(SacpId.ATTR_RIGHT_SIDE_BRUSH_CURRENT, 0)
        fan = self.db.get(SacpId.ATTR_FAN_CURRENT, 0)
        fv = self.db.get(SacpId.ATTR_FAN_SPEED, 0)
        wt = self.db.get(SacpId.ATTR_WATER_PUMP_CURRENT, 0)
        
        self.labelMainBrushCurrentValue.setText(to_float_string(mb))
        self.labelLeftBrushCurrentValue.setText(to_float_string(lb))
        self.labelRightBrushCurrentValue.setText(to_float_string(rb))
        self.labelFanCurrentValue.setText(to_float_string(fan))
        self.labelFanSpeedValue.setText(to_float_string(fv))
        self.labelWaterPumpCurrentValue.setText(to_float_string(wt))
        
    def updateGroundDetectState(self):
        f = self.db.get(SacpId.ATTR_GROUND_DETECT_FRONT, 0)
        l = self.db.get(SacpId.ATTR_GROUND_DETECT_LEFT, 0)
        r = self.db.get(SacpId.ATTR_GROUND_DETECT_RIGHT, 0)
        
        self.labelGdFrontValue.setText(to_float_string(f))
        self.labelGdLeftValue.setText(to_float_string(l))
        self.labelGdRightValue.setText(to_float_string(r))

    def updateMachineState(self):
        state = self.db.get(SacpId.ATTR_MACHINE_STATE, 0)
        
        ## 碰撞， 有碰撞，显示红色， 无碰撞绿色
        if state & SacpId.STATE_FRONT_COLLISION:
            self.ledCollisionFront.active()
        else:
            self.ledCollisionFront.inactive()
            
        if state & SacpId.STATE_LEFT_COLLISION:
            self.ledCollisionLeft.active()
        else:
            self.ledCollisionLeft.inactive()  
                  
        if state & SacpId.STATE_RIGHT_COLLISION:
            self.ledCollisionRight.active()
        else:
            self.ledCollisionRight.inactive()  

        if state & SacpId.STATE_ON_DOCK:
            self.ledOnCharger.active()
        else:
            self.ledOnCharger.inactive()  

        if state & SacpId.STATE_DUSTBIN_INPLACE:
            self.ledDustBox.active()
        else:
            self.ledDustBox.inactive()  
            
        if state & SacpId.STATE_MOP_INPLACE:
            self.ledMopState.active()
        else:
            self.ledMopState.inactive()              

        if state & SacpId.STATE_LEFT_WHEEL_OFF:
            self.ledLeftWheelOff.active()
        else:
            self.ledLeftWheelOff.inactive()           

        if state & SacpId.STATE_RIGHT_WHEEL_OFF:
            self.ledRightWheelOff.active()
        else:
            self.ledRightWheelOff.inactive()                

        if state & SacpId.STATE_WATER_TANK_INPLACE:
            self.ledWaterTank.active()
        else:
            self.ledWaterTank.inactive()              

        if state & SacpId.STATE_CHARGING:
            self.ledCharging.active()
        else:
            self.ledCharging.inactive()           

        if state & SacpId.STATE_LIDAR_COLLISION:
            self.ledCollisionLidar.active()
        else:
            self.ledCollisionLidar.inactive()   

        if state & SacpId.STATE_FRONT_GROUND_DETECT_ALARM:
            self.ledGdFront.active()
        else:
            self.ledGdFront.inactive()              

        if state & SacpId.STATE_LEFT_GROUND_DETECT_ALARM:
            self.ledGdLeft.active()
        else:
            self.ledGdLeft.inactive()           

        if state & SacpId.STATE_RIGHT_GROUND_DETECT_ALARM:
            self.ledGdRight.active()
        else:
            self.ledGdRight.inactive()   
            
        if state & SacpId.STATE_UPGRADE_REQUEST:
            self.ledUpgradeRequest.active()
        else:
            self.ledUpgradeRequest.inactive()

        
    def updateSerialPorts(self):
        """更新串口波特率列表
           只在串口未打开时处理
        """
        if self.comOpened:
            return 
        ports = self.setting.getSerialPorts()   
        self.log("检测串口列表:" + str(ports))     
        self.comboBoxComPort.clear()   
        if len(ports):
            self.comboBoxComPort.addItems(ports)            
        self.comboBoxBaudrate.clear()
        self.comboBoxBaudrate.addItems(self.setting.getSerialBaudrates())

    def onComPortChanged(self):
        ## 保存到配置
        self.setting.setSerialConfig(port=self.comboBoxComPort.currentText())

    def onComBaudrateChanged(self):
        self.setting.setSerialConfig(rate=self.comboBoxBaudrate.currentText())

    ### 串口连接处理
    def onComPortClosed(self):
        """当串口关闭时，执行一下事情 
           串口灯显示 红灯闪烁 
           关闭SACP 客户端 
           开放串口参数设置控件
        """
        self.buttonOpenCom.setText("打开")
        self.ledComState.flash(Indicator.INACTIVE, 300)
        self.comboBoxComPort.setDisabled(False)
        self.comboBoxBaudrate.setDisabled(False)
    
    def onComPortOpened(self):
        """当串口开启成功后，执行一下事情 
           串口灯显示绿灯 
           关闭串口参数设置控件
        """
        self.buttonOpenCom.setText("关闭")
        self.ledComState.active()
        self.comboBoxComPort.setDisabled(True)
        self.comboBoxBaudrate.setDisabled(True)
        
    def onDeviceActive(self):
        """
            当检测到设备时，执行此函数 
        """        
        if self.vofaServer:
            self.vofaServer.enable()

        # 更新设备版本信息
        if not self.sacpRobot.update_version_info():
            self.log("获取设备版本信息失败")
    
    def onDeviceInactive(self):
        ## 关闭VOFA服务功能
        if self.vofaServer:
            self.vofaServer.disable()

    def deviceLinkStateTimerHandle(self):
        """定时检查设备活动状态，更新设备状态
           设备指示灯有三个状态
            红色，橙色闪烁, 绿色常亮            
        """
        newState = self.deviceState        
        if not self.sacpRobot.is_running():
            newState = self.STATE_COM_CLOSED
        elif self.sacpRobot.is_alive():
            newState = self.STATE_DEVICE_ACTIVE
        else:
            newState = self.STATE_DEVICE_INACTIVE

        if newState != self.deviceState:

            if newState == self.STATE_DEVICE_ACTIVE:
                ## 未活动到设备活动
                self.ledLinkState.flash(Indicator.ACTIVE, 200)
                self.onDeviceActive()
            else:
                ## 需要区分，活动到未活动，或串口关闭
                if self.deviceState == self.STATE_DEVICE_ACTIVE: 
                    self.onDeviceInactive()
                ## 分别处理不同的情况
                if newState == self.STATE_DEVICE_INACTIVE:
                    self.ledLinkState.flash(Indicator.WARNING, 300)
                else:
                    self.ledLinkState.inactive() 
            self.deviceState = newState

        self.updateDeviceInfo()           
  
    def onButtonOpenComClicked(self):
        if not self.comOpened:
            # open sacp client 
            port = self.comboBoxComPort.currentText()
            rate = int(self.comboBoxBaudrate.currentText())            
            success = self.sacpRobot.start(port, rate)
            if success:
                self.comOpened = True 
                self.onComPortOpened()
            else :
                self.log("串口(%s/%d)打开失败" % (port, rate))
        else :
            # try stop sacp client 
            if self.sacpRobot.is_running():
                self.sacpRobot.stop()
            self.comOpened = False 
            self.onComPortClosed()
            
    ## TAB 机器控制处理子函数          
    def pwmRegultorApply(self, attrId, value):
        """PWM控制输出处理

        Args:
            attrId (_type_): _description_
            value (_type_): _description_
        """
        log.debug("pwm set attr:%d = %d" % (attrId, value))
        
        if self.deviceState != self.STATE_DEVICE_ACTIVE:
            # 不要打印，比较多
            #self.log("设备未连接，请检查")
            return 

        if not self.sacpRobot.drive_motor_pwm(attrId, value):
            self.log("PWM电机控制指令下发失败!")
        else:
            #self.log("PWM控制电机成功!")
            pass 

    def dirButtonHandle(self, key, event):
        """方向键处理
        
        Args:
            key (_type_): _description_
            event (_type_): _description_
        """
        log.debug("dir key:%d, event:%d" % (key, event))

        if self.deviceState != self.STATE_DEVICE_ACTIVE:
            # 不要打印，比较多
            #self.log("设备未连接，请检查")
            return 

    ## TAB PID控制处理子函数 
    def getPidRangeValues(self):   
        """获取当前的范围配置值

        Returns:
            _type_: 配置字典
        """
        index = self.comboBoxPidType.currentIndex()
        rangeId = 0
        try:
            rangeId = self.pidTargets[index]['range']
        except Exception as e:
            self.log("配置错误，获取范围参数失败")

        rangeValues = {}
        try:
            rangeValues = self.pidRanges[rangeId]
        except Exception as e:
            self.log("配置错误，无法获取指定范围参数，使用默认") 
        
        return rangeValues
    
    def getPidTargetType(self):
        index = self.comboBoxPidType.currentIndex()
        targetType = ''
        try:
            targetType = self.pidTargets[index]['target']
        except Exception as e:
            self.log("配置错误，获取目标参数失败")       
        return targetType

    def onPidControlTypeChanged(self, checked=False):        
        rangeValues = self.getPidRangeValues()
        try:
            lstep = rangeValues['limit_step']
            cmax = rangeValues['control_max']
        except Exception as e:
            self.log("配置错误，无法获取一些配置项")
            return 
                        
        if self.radioPid.isChecked():
            targetType = self.getPidTargetType()
            if targetType == 'left_wheel' or targetType == 'right_wheel' or targetType == 'both_wheel':
                self.pidControl = GeneralRegultor(self.dialTargetValue, self.labelTargetValue, 
                    id=1, real_min=(cmax * -1.0), real_max=cmax, real_step=lstep, reset_value=0.0)
            else:
                self.pidControl = GeneralRegultor(self.dialTargetValue, self.labelTargetValue, 
                    id=1, real_min=0.0, real_max=cmax, real_step=lstep)                
        else:
            self.pidControl = GeneralRegultor(self.dialTargetValue, self.labelTargetValue, 
                id=1, real_min=0.0, real_max=100.0, real_step=1.0)

        self.pidControl.valueChanged.connect(self.onPidControlValueChanged)
        self.buttonResetDial.clicked.connect(self.pidControl.reset)

    def onPidTargetChanged(self, index):        
        self.log("修改PID调试目标:%s(%d)" % (self.pidTargets[index]['name'], index))
        rangeValues = self.getPidRangeValues()
        try:                        
            kmin = rangeValues['kx_min']
            kmax = rangeValues['kx_max']
            kstep = rangeValues['kx_step']        
            lmin = rangeValues['limit_min']
            lmax = rangeValues['limit_max']
            lstep = rangeValues['limit_step']
            cmax = rangeValues['control_max']
        except Exception as e:
            self.log("配置错误，无法获取一些配置项")
            return 

        self.pidKp = GeneralRegultor(self.sliderKp, self.labelKpValue, 
            buttonPlus=self.buttonKpPlus, buttonMinus=self.buttonKpMinus, 
            id=1, real_min=kmin, real_max=kmax, real_step=kstep)

        self.pidKi = GeneralRegultor(self.sliderKi, self.labelKiValue, 
            buttonPlus=self.buttonKiPlus, buttonMinus=self.buttonKiMinus, 
            id=1, real_min=kmin, real_max=kmax, real_step=kstep)
        
        self.pidKd = GeneralRegultor(self.sliderKd, self.labelKdValue, 
            buttonPlus=self.buttonKdPlus, buttonMinus=self.buttonKdMinus, 
            id=1, real_min=kmin, real_max=kmax, real_step=kstep)
                
        self.pidLimited = GeneralRegultor(self.sliderMaxTarget, self.labelMaxTargetValue, 
            buttonPlus=self.buttonMaxTargetPlus, buttonMinus=self.buttonMaxTargetMinus, 
            id=1, real_min=lmin, real_max=lmax, real_step=lstep)
        
        self.onPidControlTypeChanged()
        
    def onPidControlValueChanged(self, id, value):
        """PID控制输出处理函数

        Args:
            id (_type_): _description_
            value (_type_): _description_
        """
        targetType = self.getPidTargetType()
        controlType = "pid"
        if self.radioPwm.isChecked():
            controlType = "pwm"
        
        log.debug("set %s %s:%.3f" % (targetType, controlType, value))
        
        if self.deviceState != self.STATE_DEVICE_ACTIVE:
            # 不要打印，比较多
            #self.log("设备未连接，请检查")
            return 

        if targetType == 'fan':
            if controlType == 'pwm':
                self.sacpRobot.drive_fan_pwm(value)
            else :
                self.sacpRobot.drive_fan_speed(value)
        elif targetType == 'left_wheel':            
            if controlType == 'pwm':
                self.sacpRobot.drive_wheel_pwm(left=value)
            else:
                self.sacpRobot.drive_wheel_velocity(left=value)
        elif targetType == 'right_wheel':            
            if controlType == 'pwm':
                self.sacpRobot.drive_wheel_pwm(right=value)
            else:
                self.sacpRobot.drive_wheel_velocity(right=value)
        elif targetType == 'both_wheel':            
            if controlType == 'pwm':
                self.sacpRobot.drive_wheel_pwm(left=value, right=value)
            else:
                self.sacpRobot.drive_wheel_velocity(left=value, right=value)                        

    def tryReadPidParameters(self):
        # 读机器参数
        if self.deviceState != self.STATE_DEVICE_ACTIVE:
            self.log("设备未连接，请检查")
            return 
        
        t = self.getPidTargetType()
        
        if t == "fan":
            if not self.sacpRobot.get_fan_pid():
                self.log("读取风机PID参数失败!")
                return 
            self.log("读取风机PID参数成功!")
            ## 更新这几个值 
            kp = self.db.get(SacpId.ATTR_FAN_PID_KP, 0)
            ki = self.db.get(SacpId.ATTR_FAN_PID_KI, 0)
            kd = self.db.get(SacpId.ATTR_FAN_PID_KD, 0)
            pl = self.db.get(SacpId.ATTR_FAN_PID_LIMITED, 0)
            
            self.pidKp.setValue(kp)
            self.pidKi.setValue(ki)
            self.pidKd.setValue(kd)
            self.pidLimited.setValue(pl)
        
        else:
            if not self.sacpRobot.get_wheel_pid():
                self.log("读取主轮PID参数失败!")
                return 
            self.log("读取主轮PID参数成功!")
            ## 更新这几个值 
            kp = self.db.get(SacpId.ATTR_WHEEL_PID_KP, 0)
            ki = self.db.get(SacpId.ATTR_WHEEL_PID_KI, 0)
            kd = self.db.get(SacpId.ATTR_WHEEL_PID_KD, 0)
            pl = self.db.get(SacpId.ATTR_WHEEL_PID_LIMITED, 0) 
            
            self.pidKp.setValue(kp)
            self.pidKi.setValue(ki)
            self.pidKd.setValue(kd)
            self.pidLimited.setValue(pl)                        


    def tryWritePidParameters(self):
        if self.deviceState != self.STATE_DEVICE_ACTIVE:
            self.log("设备未连接，请检查")
            return 
        
        kp = self.pidKp.getValue()
        ki = self.pidKi.getValue()
        kd = self.pidKd.getValue()
        pl = self.pidLimited.getValue()
        
        t = self.getPidTargetType()
        
        if t == "fan":            
            if not self.sacpRobot.set_fan_pid(kp, ki, kd, pl):
                self.log("设置风机PID参数失败!")
                return 
            self.log("设置风机PID参数成功!")        
        else:
            if not self.sacpRobot.set_wheel_pid(kp, ki, kd, pl):
                self.log("设置主轮PID参数失败!")
                return 
            self.log("设置主轮PID参数成功!")
            
    def onButtonTxPauseClicked(self):
        if self.deviceState != self.STATE_DEVICE_ACTIVE:
            self.log("设备未连接，请检查")
            return 
        if self.vofaServer.is_enabled():
            # 如果已启用，设置为关闭
            self.buttonDataTxPause.setText("开启")
            self.vofaServer.disable()
        else:
            self.buttonDataTxPause.setText("暂停")
            self.vofaServer.enable()            

    ## TAB 数据可视化处理子函数 
    def setupDataVisualChannel(self, obj:QtWidgets.QComboBox, ids, options, attrId):
        obj.addItems(options)
        for index in range(len(ids)):
            if ids[index] == attrId:
                obj.setCurrentIndex(index)
                break
        obj.currentIndexChanged.connect(self.onDataVisualChannelConfigChanged)

    def saveDataVisualConfig(self):
        try:
            port = int(self.editTcpPort.text())
            period = int(self.editPeriod.text())
        except Exception as e:            
            self.dataVisualConfigTimer.stop()
            return 
        ## 获取每个channel配置值
        chs = []
        chs.append(self.dataVisualAttributeIds[self.comboBoxDataCh1.currentIndex()])
        chs.append(self.dataVisualAttributeIds[self.comboBoxDataCh2.currentIndex()])
        chs.append(self.dataVisualAttributeIds[self.comboBoxDataCh3.currentIndex()])
        chs.append(self.dataVisualAttributeIds[self.comboBoxDataCh4.currentIndex()])
        chs.append(self.dataVisualAttributeIds[self.comboBoxDataCh5.currentIndex()])
        chs.append(self.dataVisualAttributeIds[self.comboBoxDataCh6.currentIndex()])
        chs.append(self.dataVisualAttributeIds[self.comboBoxDataCh7.currentIndex()])
        chs.append(self.dataVisualAttributeIds[self.comboBoxDataCh8.currentIndex()])
        self.setting.setDataVisualConfig(port, period, chs)
        self.dataVisualConfigTimer.stop()

    def onDataVisualIpPortChanged(self, text:str):
        if text.isdecimal():
            self.dataVisualConfigTimer.stop()
            self.dataVisualConfigTimer.start(1000)
        else:
            log.info("请输入合法数字")

    def onDataVisualIntervalChanged(self, text:str):
        if text.isdecimal():
            self.dataVisualConfigTimer.stop()
            self.dataVisualConfigTimer.start(1000)
        else:
            log.info("请输入合法数字")

    def onDataVisualChannelConfigChanged(self, index=0):
        self.dataVisualConfigTimer.stop()
        self.dataVisualConfigTimer.start(1000)

    def onDataVisualEnableChanged(self, checked=False):
        en = not self.checkBoxDataVisual.isChecked()
        self.editTcpPort.setEnabled(en)
        self.editPeriod.setEnabled(en)
        self.comboBoxDataCh1.setEnabled(en)
        self.comboBoxDataCh2.setEnabled(en)
        self.comboBoxDataCh3.setEnabled(en)
        self.comboBoxDataCh4.setEnabled(en)
        self.comboBoxDataCh5.setEnabled(en)
        self.comboBoxDataCh6.setEnabled(en)
        self.comboBoxDataCh7.setEnabled(en)
        self.comboBoxDataCh8.setEnabled(en)

        if self.vofaServer != None:
            self.vofaServer.stop()

        if self.checkBoxDataVisual.isChecked():
            port = self.setting.getDataVisualIpPort()
            period = self.setting.getDataVisualTxPeriod()
            chAttrIds = self.setting.getDataVisualChannelConfigs()
            self.vofaServer = VofaServer(self.db, attrIds=chAttrIds, uiLog=self.uiLog, port=port, period=period)
            self.vofaServer.start()
            ## 如果设备已连接，启用数据发送
            if self.deviceState == self.STATE_DEVICE_ACTIVE:
                self.vofaServer.enable()

    def tryReadMachineParameters(self):
        # 读机器参数
        if self.deviceState != self.STATE_DEVICE_ACTIVE:
            self.log("设备未连接，请检查")
            return 
        
        if not self.sacpRobot.get_machine_parameters():
            self.log("读取机器参数失败!")
        else:
            self.log("读取机器参数成功!")   
             
        if not self.sacpRobot.get_ground_detect_threshold():
            self.log("读取地检阈值失败!")
        else:
            self.log("读取地检阈值成功!")           
        
        ## 更新这几个值 
        wd = self.db.get(SacpId.ATTR_WHEEL_DIAMETER, 0)
        gr = self.db.get(SacpId.ATTR_WHEEL_GEAR_RATIO, 0)
        ea = self.db.get(SacpId.ATTR_WHEEL_ENCODER_ACCURACY, 0)
        ts = self.db.get(SacpId.ATTR_WHEEL_TRACK_SIZE, 0)
        fs = self.db.get(SacpId.ATTR_FAN_SPEED_RATIO, 0)
        
        self.editWheelDiameter.setText(to_float_string(wd))
        self.editGearRatio.setText(to_float_string(gr))
        self.editEncoderAccuracy.setText(to_float_string(ea))
        self.editWheelTrack.setText(to_float_string(ts))
        self.editFanSpeedRatio.setText(to_float_string(fs))     
        
        ft = self.db.get(SacpId.ATTR_GROUND_DETECT_FRONT_THRESHOLD, 0)
        lt = self.db.get(SacpId.ATTR_GROUND_DETECT_LEFT_THRESHOLD, 0)
        rt = self.db.get(SacpId.ATTR_GROUND_DETECT_RIGHT_THRESHOLD, 0)
        
        self.editGdFrontThreshold.setText(to_int_string(ft))
        self.editGdLeftThreshold.setText(to_int_string(lt))
        self.editGdRightThreshold.setText(to_int_string(rt))

    def tryWriteMachineParameters(self):
        if self.deviceState != self.STATE_DEVICE_ACTIVE:
            self.log("设备未连接，请检查")
            return 
        
        mp = MachineParameter()
        mp.wheel_diameter = get_float(self.editWheelDiameter.text())
        mp.wheel_gear_ratio = get_float(self.editGearRatio.text())
        mp.wheel_encoder_accuracy = get_float(self.editEncoderAccuracy.text())
        mp.wheel_track_size = get_float(self.editWheelTrack.text())
        mp.fan_speed_ratio = get_float(self.editFanSpeedRatio.text())

        if not self.sacpRobot.set_machine_parameters(mp):
            self.log("设置机器参数失败!")
        else:       
            self.log("设置机器参数成功!")
        
        ft = get_int(self.editGdFrontThreshold.text())
        lt = get_int(self.editGdLeftThreshold.text())
        rt = get_int(self.editGdRightThreshold.text())

        if not self.sacpRobot.set_ground_detect_threshold(ft, lt, rt):
            self.log("设置地检阈值失败!")
        else:       
            self.log("设置地检阈值成功!")        

if __name__=="__main__":
    app = QApplication(sys.argv)
    win = MainWindow()
    win.show()
    sys.exit(app.exec())

